/*! \file lqregulator_motor2.cpp

\brief linear quadratic regulator for Cartpole pendulum system

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
*/
/*
INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate

*/

#include "../../dynlib.h"

void controller(Matrix &SimParam,Matrix &X,int i,int ui,double *Tau,ColumnVector &dX)
{
	RowVector k(4);

	k(1)=-1.0;
	k(2)=-77.0441;
	k(3)=-4.1541;
	k(4)=-12.2634;
	//k[] = { -1.0,-77.0441,-4.1541,-12.2634 }
	
	*Tau=-dotproduct(k,X.column(i).t());
}
